/*
 * kalman.c
 *
 *  Created on: 10-12-2012
 *      Author: Maciej
 */

#include "kalman.h"

/// Initializes filter
///
///
void Kalman_Init(sKalmanFilter* filter)
{
	// 0 -acc

	// 15 -15

	filter->K[0] 	= 0;
	filter->K[1] 	= 0;

	filter->P[0][0] = 0;
	filter->P[0][1] = 0;
	filter->P[1][0] = 0;
	filter->P[1][1] = 0;
	filter->bias 	= 0;
	filter->rate 	= 0;
	filter->angle 	= 0;


	filter->Q[0] 	= 200;
	filter->Q[1] 	= 1;
	filter->R_measure = 200;
}

/// Performs filter update
///
///
float Kalman_Process(sKalmanFilter* filter, float newAngle, float newRate, float measurement_rate)
{
	// I - predict

	// Step 1
	filter->rate = newRate - filter->bias;
	filter->angle += filter->rate / measurement_rate;

	// Update estimation error covariance - Project the error covariance ahead
	// Step 2
	filter->P[0][0] += (filter->P[1][1] / measurement_rate - filter->P[0][1] - filter->P[1][0] + filter->Q[0])/measurement_rate;
	filter->P[0][1] -= filter->P[1][1]/measurement_rate;
	filter->P[1][0] -= filter->P[1][1]/measurement_rate;
	filter->P[1][1] += filter->Q[1]/measurement_rate;

	// Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
	// Calculate Kalman gain - Compute the Kalman gain
	// Step 4
	filter->S = filter->P[0][0] + filter->R_measure;
	// Step 5
	filter->K[0] = filter->P[0][0] / filter->S;
	filter->K[1] = filter->P[1][0] / filter->S;

	// Calculate angle and bias - Update estimate with measurement zk (newAngle)
	/* Step 3 */
	filter->y = newAngle - filter->angle;
	/* Step 6 */
	filter->angle += filter->K[0] * filter->y;
	filter->bias += filter->K[1] * filter->y;

	// Calculate estimation error covariance - Update the error covariance
	/* Step 7 */
	filter->P[0][0] -= filter->K[0] * filter->P[0][0];
	filter->P[0][1] -= filter->K[0] * filter->P[0][1];
	filter->P[1][0] -= filter->K[1] * filter->P[0][0];
	filter->P[1][1] -= filter->K[1] * filter->P[0][1];

	return filter->angle;
}
